
#include "constraints.h"

#include <cassert>
#include <cmath>
#include <algorithm>

Constraints::Constraints() {
   
   reserve(50);
}

Constraints::~Constraints() {
   
   this->clear();
}

const Constraints& Constraints::operator=(const Constraints &c) {
   
   this->clear();
   this->positions.resize(c.size());
   this->ids.resize(c.size());
   for (unsigned int i=0; i<positions.size(); ++i) {
      this->positions[i] = c.positions[i];
      this->ids[i] = c.ids[i];
   }
   return *this;
}
   
void Constraints::push_back(const Point3 &p) {
   
   positions.push_back(p);
   ids.push_back(-1);
}

void Constraints::push_back(const Point3 &p, int id) {
   
   positions.push_back(p);
   ids.push_back(id);
}

void Constraints::reserve(unsigned int n) {
   
   positions.reserve(n);
   ids.reserve(n);
}
   
void Constraints::erase(unsigned int i) {
   
   assert(i<size());
   
   std::vector<Point3>::iterator it = positions.begin();
   advance(it, i);
   positions.erase(it);
   
   std::vector<int>::iterator itid = ids.begin();
   advance(itid, i);
   ids.erase(itid);
}
   
const Number* Constraints::getPositionPointer(unsigned int i) const { 
   return &positions[i][0]; 
}
   
Point3 Constraints::getPosition(unsigned int i) const {
   return positions[i]; 
}
   
void Constraints::setPosition(unsigned int i, const Point3 &p) {
   positions[i] = p; 
}
   
// TRANSFORMATIONS FUNCTIONS

void Constraints::transform(Number m[]) {
   
   Number x, y, z;
   for (unsigned int i=0; i<positions.size(); ++i) {
      Point3 &p = positions[i];
      
      x = m[0]*p[0] + m[4]*p[1] + m[8] *p[2] + m[12];
      y = m[1]*p[0] + m[5]*p[1] + m[9] *p[2] + m[13];
      z = m[2]*p[0] + m[6]*p[1] + m[10]*p[2] + m[14];
      p = Point3(x, y, z);
   }
}

void Constraints::rotate(const Point3 &o, const Vector3 &u, Number angle) {
   
   for (unsigned int i=0; i<positions.size(); ++i) {
      Point3 &p = positions[i];
      p = p - Vector3(o[0], o[1], o[2]);
      Vector3 vp(p[0], p[1], p[2]);  /// vp = p - Vector3(0,0,0)
      vp = vp*cos(angle) + cross_product(u,vp)*sin(angle) + (u*vp)*u*(1.0-cos(angle));
      p = Point3(vp[0],vp[1],vp[2]);
      p = p + Vector3(o[0], o[1], o[2]); 
   }
}
   
void Constraints::translate(const Vector3 &t) {
   
   for (unsigned int i=0; i<positions.size(); ++i) {
      Point3 &p = positions[i];
      p = p + t;
   }
}

void Constraints::transform(unsigned int k, Number m[]) {
   
   Point3 &p = positions[k];
   Number x = m[0]*p[0] + m[4]*p[1] + m[8] *p[2] + m[12];
   Number y = m[1]*p[0] + m[5]*p[1] + m[9] *p[2] + m[13];
   Number z = m[2]*p[0] + m[6]*p[1] + m[10]*p[2] + m[14];
   p = Point3(x, y, z);
}
   
void Constraints::rotate(unsigned int k, const Point3 &o, const Vector3 &u, Number angle) {
   
   Point3 &p = positions[k];
   p = p - Vector3(o[0], o[1], o[2]);
   Vector3 vp(p[0], p[1], p[2]);  /// vp = q - Vector3(0,0,0)
   vp = vp*cos(angle) + cross_product(u,vp)*sin(angle) + (u*vp)*u*(1.0-cos(angle));
   p = Point3(vp[0],vp[1],vp[2]);
   p = p + Vector3(o[0], o[1], o[2]);
}
   
void Constraints::translate(unsigned int k, const Vector3 &t) {
   
   Point3 &p = positions[k];
   p = p + t;
}
   
void Constraints::transform(const std::vector<unsigned int> &g, float m[]) {
   
   for (unsigned int i=0; i<g.size(); ++i) {
      Point3 &p = positions[g[i]];
      Number x = m[0]*p[0] + m[4]*p[1] + m[8] *p[2] + m[12];
      Number y = m[1]*p[0] + m[5]*p[1] + m[9] *p[2] + m[13];
      Number z = m[2]*p[0] + m[6]*p[1] + m[10]*p[2] + m[14];
      p = Point3(x, y, z);
   }
}
   
void Constraints::rotate(const std::vector<unsigned int> &g, const Point3 &o, const Vector3 &u, Number angle) {
   
   for (unsigned int i=0; i<g.size(); ++i) {
      Point3 &p = positions[g[i]];
      p = p - Vector3(o[0], o[1], o[2]);
      Vector3 vp(p[0], p[1], p[2]);  /// vp = q - Vector3(0,0,0)
      vp = vp*cos(angle) + cross_product(u,vp)*sin(angle) + (u*vp)*u*(1.0-cos(angle));
      p = Point3(vp[0],vp[1],vp[2]);
      p = p + Vector3(o[0], o[1], o[2]);
   }
}
   
void Constraints::translate(const std::vector<unsigned int> &g, const Vector3 &t) {
   
   for (unsigned int i=0; i<g.size(); ++i) {
      Point3 &p = positions[g[i]];
      p = p + t;
   }
}
